﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Axiom.Math;
using Axiom.Core;
using CommonClasses.Utility;

namespace VisualDM.Model
{
    public static class AxiomUtility
    {
        public static Quaternion Multiply(Matrix4 matrix, Quaternion quater)
        {
            Vector4 result = Vector4.Zero;
            Vector4 copy = new Vector4(quater.w, quater.z, quater.y, quater.z);
            
            for (int i = 0; i < 4; i++)
                for (int j = 0; j < 4; j++)
                    result[i] += matrix[i, j] * copy[j];

            float resultW = 1.0f / (matrix.m30+matrix.m31+matrix.m32+matrix.m33);

            return new Quaternion(resultW, result.x * resultW, result.y * resultW, result.z * resultW);

            /*float resultW = 1.0f / (result.w + result.x + result.y + result.z);

            return new Quaternion(resultW,//result.w * inverseW, 
                result.x * resultW,
                result.y * resultW,
                result.z * resultW);*/
/*            var copy = new Vector3(quater.x, quater.y, quater.z);
            var result = matrix * copy;
            var w = 1.0f;
            return new Quaternion(w,
                result.x,
                result.y,
               result.z);*/

        }

        /// <summary>
        /// Получить вектор вида (Pitch, Yaw, Roll) - глобальные углы относительно координатных осей
        /// </summary>
        /// <param name="node">Узел сцены</param>
        /// <returns></returns>
        public static Vector3 GetGlobalOrientation(this SceneNode node)
        {
            var result = Vector3.Zero;
            //x - pitch
            //y - yaw
            //z - roll

            node.DeepWalk("Parent",
                (obj) =>
                {
                    var current = obj as SceneNode;
                    result.x += current.Orientation.Pitch;
                    result.y += current.Orientation.Yaw;
                    result.z += current.Orientation.Roll;
                });

            return result;
        }
        /// <summary>
        /// sn - привязываемый
        /// s - к кому привязываем
        /// </summary>
        /// <param name="sn"></param>
        /// <param name="?"></param>
        /// <returns></returns>
        public static Quaternion GetOrientationWithTransform(SceneNode sn, SceneObject s)
        {
            #region Преобразование ориентации

            //получаем глобальную ориентацию привязываемого
            var dependentGlobalOrient = sn.GetGlobalOrientation();
            //получаем глобальную ориенатцию нового предка
            var newParentGlobalOrient = s.SceneNode.GetGlobalOrientation();
            //находим новую относительную ориентацию привязываемого
            var difference = newParentGlobalOrient - dependentGlobalOrient;
            return Quaternion.FromEulerAngles(difference.x - difference.z,
                difference.y/* + (float)Math.PI / 2 */- difference.z,
                - difference.z + (float)Math.PI / 2);//(difference.x + difference.y));

            #endregion
        }
    }
}
